#include "CSVConverter.h"
#include <fstream>
#include <MMM/XMLTools.h>
#include <MMM/MMMCore.h>
#include <boost/algorithm/string.hpp>

using namespace MMM;

CSVConverter::CSVConverter() : IMUConverter()
{
}

float CSVConverter::toSeconds(const std::string &timestamp) {
    std::vector<std::string> values;
    boost::split(values, timestamp, boost::is_any_of(":"));
    float timestampSeconds = XML::convertToFloat(values[2].c_str()) + XML::convertToFloat(values[1].c_str()) * 60.0f + XML::convertToFloat(values[0].c_str()) * 60.0f * 60.0f;
    return timestampSeconds;
}

std::vector<IMUDataPtr> CSVConverter::convertIMUData(const std::string &filePath, float timeOffset) {
    std::ifstream infile(filePath);
    std::string line;
    std::getline(infile, line); // Ignore first line
    std::vector<IMUDataPtr> imuData;
    int lineCount = 0;
    while (std::getline(infile, line))
    {
        lineCount++;
        std::vector<std::string> values;
        boost::split(values, line, boost::is_any_of(","));
        Eigen::Vector3f acceleration;
        float timestep = 0.0f;

        try {
            acceleration << XML::convertToFloat(values[2].c_str()), XML::convertToFloat(values[3].c_str()), XML::convertToFloat(values[4].c_str());
            float timestampSeconds = toSeconds(values[0]);
            if (lineCount == 1) startTimestampSeconds = timestampSeconds;
            else timestep = timestampSeconds - startTimestampSeconds;
        } catch (Exception::XMLFormatException& e) {
            MMM_ERROR << e.what() <<  "Ignoring this measurement..." << std::endl;
            continue;
        }

        if (timestep < 0) {
            MMM_ERROR << "Timestamp in Line " << lineCount << " is not correct! Ignoring this measurement..." << std::endl;
            continue;
        }

        IMUDataPtr data(new IMUData(timestep, acceleration));
        imuData.push_back(data);
    }

    return imuData;
}
